/*****************************************************************************/
/* 
 * FILENAME			: hs.c
 *
 * DESCRIPTION		: Main loop of Hitec Aurora 9 Sensor Station clone
 *
 * PUBLIC FUNCTIONS : -
 *
 * NOTES 			: - 
 * 
 * AUTHOR			: Diederich Kroeske (dkroeske@d68.nl)
 *
 * VERSION INFO		: V1.0	dkroeske		18Aug2011	Released version.
 *					  v1.1  dkroeske		09NOV2011   Added RPM support
 * 					  v1.2	dkroeske		09MRT2012	Bugfixes - reading voltages
 *																 - GPS coords
 */
 /*****************************************************************************/

#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/twi.h>
#include <util/delay.h>

#include "usart.h"
#include "led.h"
#include "mediatek.h"

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


// defines
#define I_IN	0
#define U_IN1	1
#define U_IN2	2
#define U_OPT	3

/* ********************************************************* */
/* 
 * Define Sensor Configurations
 */
 
// Select TEMP sensor
#define LM335
//#define LM35

// Select CURRENT sensor
#define ACS756
//#define ATTOPILOT_90AMP

// Define Voltage measurement input
#define VOLTAGEDIRECT
//#define ATTOPILOT_90VOLT

// Prototypes
void i2c_init_sr(unsigned char slaveAddr);

typedef struct s
{
	char data[8][7];
} MSGBLOCK_STRUCT;

GPRMC_STRUCT gpsrmc;
GPGGA_STRUCT gpsgga;

MSGBLOCK_STRUCT msgs[2] = {
	{
		{{ 0x11, 0xAF, 0x00, 0x2D, 0x00, 0x00, 0x11 },
		{ 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12 },
		{ 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13 },
		{ 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14 },
		{ 0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15 },
		{ 0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16 },
		{ 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17 },
		{ 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18 }}
	},
	{
		{{ 0x11, 0xAF, 0x00, 0x2D, 0x00, 0x00, 0x11 },
		{ 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12 },
		{ 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13 },
		{ 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14 },
		{ 0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15 },
		{ 0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16 },
		{ 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17 },
		{ 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18 }}
	}
}; 

// Global variables used in hs.c
MSGBLOCK_STRUCT *pMsgWr = &msgs[0];
MSGBLOCK_STRUCT *pMsgRd = &msgs[1];
char TWIsbusy = 0;
uint16_t msCnt;
uint8_t signalStrength = 0;
uint16_t Iref;

// Used in the Input Capture ISR
volatile uint16_t tcOldVal;
uint16_t tcSamples[8];
uint8_t tcidx;
uint16_t tcResult;

// Used to median filter current when using AttoPilot sensor
#define AP_CURRENT_MAX_SAMPLES 3
uint16_t apCurrentSamples[AP_CURRENT_MAX_SAMPLES];
uint8_t apIndex;

// function prootypes
void initLed(void);
void initADC(void);
uint16_t sampleChannel(uint8_t ch);
void setTemp(uint8_t nr, uint8_t value);
void setRpm(uint8_t nr, uint16_t value);
void setVoltage(uint16_t value);
void setCurrent(uint16_t value);
void setGPSValue(GPRMC_STRUCT *gprmc, GPGGA_STRUCT *gpsgga);
uint16_t medianFilter3(uint16_t *buf);


/*****************************************************************************/
void initADC(void)
/*
 * Description	: Init the 10 bits ADC
 * input		: -
 * output		: -
 * 
 * (1) URef is used as reference. Assumed this is 3.3V
 * (2) ...
 */
/*****************************************************************************/
{
	ADMUX = 0x00;	
	ADCSRA = ((1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0));
}


/*****************************************************************************/
uint16_t sampleChannel(uint8_t ch)
/*
 * Description	: sample the 10 bits ADC
 * input		: (uint8_t ch samples ADC channel I_IN, U_IN1, U_IN2 en U_OPT
 * output		: -
 * 
 * (1) this function is a busy waiting.
 * (2) return ADC. It's value depends on Uref.
 */
/*****************************************************************************/
{
	uint16_t adcVal;	
	
	ADMUX &= ~0x0F;
	switch(ch)
	{
		case I_IN:	// ADC0
			break;
		case U_IN1:	// ADC1
			ADMUX |= (1<<MUX0);
			break;
		case U_IN2:	// ADC2
			ADMUX |= (1<<MUX1);
			break;
		case U_OPT:	// ADC3
			ADMUX |= (1<<MUX1) | (1<<MUX0);
			break;
		default:
			break;
	}
	
	// Start ADC for the selected channel and return value;
 	ADCSRA |= (1<<ADSC);				// Start ADC
 	while( (ADCSRA & (1<<ADSC)) ); 		// Wait till ready 
 	adcVal = ADCL;
 	adcVal += (ADCH << 8);

	return adcVal;
}


char buf[10];

/*****************************************************************************/
int main(void)
/*
 * Description	: Main loop.
 * input		: -
 * output		: -
 * 
 * Slow processes like temperature, voltage and current monitoring are performed
 * in the main loop. Fast (unsynchronized) events like i2c en serial rx are handled
 * in ISR's
 */
/*****************************************************************************/
{
	uint8_t idx;
	uint16_t adc;
	uint16_t temp;
		
	// Disable global interrupts
	cli();
	
	// Init Timer 0
	TCCR0 = 0x05; //0x08;
	TIMSK |= (1 << TOIE0);
	TCNT0 = 0xFF - 156;		// isr every 20 ms
	TIFR |= (1 << TOV0);

	// Init led and switch led off
	initLed();

	// Init USART 
	
	// Init GPS
	initGPS();
	// init_uart();

	// Init adc
	initADC();
	
	// Determine Iref (0 Amp situation).
	Iref = sampleChannel(I_IN);
	Iref -= 3;	

	// Init the i2c link to the optima 7/9 receivers.
	i2c_init_sr(0x10);
	TWCR |= (1<<TWINT); // clear interrupt flag, allow TW interrupts
	
	// 09 nov 2011: added input capture to measure RPM's
	TCCR1B = ((1<<CS12) | (1<<CS10)); 	// Interrupt falling edge, 
										// prescaler is clock/1024 = 7.813 MHz
	TIMSK |= (1<<TICIE1);				// Enable capture interrupts
	
	// From this point on allow interrupts
	sei();

	// Indicate board startup by flashing 3x led
	for( idx = 0; idx < 6; idx++)
	{
		toggleLed();
		_delay_ms(150);
	}
	setLedOff();

	// Assume GPS off / not connected. Tell A9 gps signal is zero
	pMsgWr->data[6][3] = 0x00; 
		
	// Enter the forever-loop
	while(1==1)
	{ 		

// Temp monitoring section, add sensors if you like.
#ifdef LM335
 		// Calculate temperature. LM335 is connected to PC3 by R/2R divider
 		// LM335 gives 10mV/K so with R/2R divider and Uref = 3.3V: 
 		// U(lm335) = 2 * (3.3/1024) * adc
 		// Temp(kelvin) = U(lm335) / 10mV
 		// Temp(Celcius) = Temp(kelvin) - 273.15
 		// Temp(Celcium) = adc * 0.6445 - 271.15;
		temp = sampleChannel(U_OPT);
		setTemp( 1, (uint8_t) ((temp * 0.6445f) - 273.15f) );
#endif

#ifdef LM35
 		// Calculate temperature. LM35 is connected directly U_IN2. U_IN2
 		// is choosen because a 10k resistor in serie protects ADC. Make sure 
 		// not to mount R11 (marked as 'opt' on PCB)
 		// LM35 gives 10mV/C starting from 0 (no negative temp, see datasheet) 
 		// With Uref = 3.3V:
 		// U(lm35) = Uref/2^10 = (3.3/1024) * adcValue
 		// Temp(celcius) = U(lm35) / 0.01 (0.01 = 10mV)
 		// Temp(Celcium) = (adcValue * 3.3/1024) * (1/0.01) = adcValue * 0.32225;
		temp = sampleChannel(U_IN2);
		setTemp( 1, (uint8_t) (temp * 0.32225f) );
		__give_compiler-error
#endif 
 		 		
// Voltage monitoring section	
#ifdef ATTOPILOT_90VOLT
 		// ADC conversion for voltage measurement, using OTTOPILOT sensor
 		// --- MAKE SURE NOT TO INSTALL R12 ---
 		// A9 expects U_IN times 10. So:
 		// Uadc = ADCvalue * (3.3 * 10)/2^10 and Uin = 63.69mV/V 
 		// Uin = ((3.3 * 10)/1024) * (1/63.69mV) * ADCvalue = 0.505991 * ADCValue
		setVoltage((uint16_t)(sampleChannel(U_IN2) * 0.505991f));
		__give_compiler-error
#endif

#ifdef VOLTAGEDIRECT
 		// ADC conversion for voltage measurement, direct
 		// U_IN1 divided by 1k/(1k+10k). Uref = 3.3V
 		// A9 expects U_IN times 10. So
 		// Uin = adc * (3.3 * 11 * 10)/2^10	= adc * 0.3545f	
		setVoltage((uint16_t)(sampleChannel(U_IN1) * 0.3545f));
#endif

// Current monitoring section 
#ifdef ACS756
 		// Measure Current. The ACS756 gives 40mV/Amp. At 0 (zero)
 		// amp this sensor goves Vcc/2. This 0 A value is measured
 		// at powerup to compensate Vcc fluctuations.
 		// Amps are send to A9 in Amps*10 format.
 		// 
 		// ((1/2^10)*3.3)/0.04 = 0.80566f where 3.3 - Uref
 		//
 		// Note that Uref = 3.3 meaning ADC clips at approx 23 Amp. Use
 		// Uref = 5 V for greater range.
 		adc = sampleChannel(I_IN);
 		adc -= Iref;
 		setCurrent((uint16_t)((adc*0.80566f)+0.5f));
#endif

#ifdef ATTOPILOT_90AMP
		// Measure current using Compact DC Voltage and Current Sense PCB
		// 90Amp version gives 36.60mV/Amp
		// Uadc = ((3.3 * 10)/1024)*ADCvalue and I = Uadc/36.6mV
		// I = ((3.3 * 10)/1024)*(1/36.6mV)*ADCvalue = 0.8805*ADCvalue
		adc = sampleChannel(U_IN1);
		
		// Store in array, simple sort and get the median (middle) so extremes 
		// fall to array boundaries. 
		apCurrentSamples[apIndex] = adc;
		apIndex++;
		apIndex%=AP_CURRENT_MAX_SAMPLES;
		
		adc = medianFilter3(apCurrentSamples);
		
		// debug
		//utoa(adc, buf, 10);
		//txstr(buf);
		//tx(10);
		//tx(13);
		
 		setCurrent( (uint16_t)(adc*0.8805f));
		__give_compiler-error
#endif
 		 				
		// Set RPMs
		// tcval equals the time between 2 falling edges on the capture input pin
		// tcval values are in 8MHz/1024 = 7812kHz = 128us time tick. E.g. if tcval
		// equals the value 678(d) it means that the time between 2 falling edges (=
		// 1/f) equals: tcval = 678 * 128uS = 8678ms = 11.52Hz. This equals 691 rpms.
		// 
		// rpms = tcval * 1024/8MHz * 60 = 1/tcval * 468750
		
		// RPM is filtered.
		tcResult = 0;
		for(idx=0; idx < 8; idx++)
		{
			tcResult += tcSamples[idx];
		}
		tcResult >>=3;
		setRpm(1, (uint16_t)(468750/tcResult));

 		// flash the led to indicate we're alive
 		setLedOn();
 		_delay_ms(50);
 		setLedOff();
 		
 		// Test if there is a GPS fix. Check status of gprmc.
 		if( gpsrmc.status )
 		{
 			pMsgWr->data[6][3] = 0x0A;
 		}
 		else
 		{
 			pMsgWr->data[6][3] = 0x00; 
 		}
 		
 		//Looptime approx 1 sec
 		for( idx = 0; idx < 20; idx++)
 		{
 			_delay_ms(40);
 		}
 		
 		
		 		  		
// 		
// Fill datablock to 
// 		
// Flip pointers is ISR != busy
// //		if( !TWIsbusy )
// //		{
// //			pMsg = pMsgWr;
// //			pMsgWr = pMsgRd;
// //			pMsgRd = pMsg;
// //		}

	}

	return 0;
}

/*****************************************************************************/
uint16_t medianFilter3(uint16_t *buf)
/*
 * Description	: Returns the value in the middle of the input buffer. 
 * 				  (This buffer probably holds adc samples)
 *				  e.g. "12 13 40" returns "13" 
 *					   "13 40 12" returns "13"
 *					   "40 12 15" returns "15" 
 *					   the value "40" is filtered out
 * input		: -
 * output		: -
 * 
 *	Non destructive on input buffer
 */
{
	uint16_t a, b, c, temp;
	
	// 3 line copy
	a = buf[0]; b = buf[1]; c = buf[2];
	
	if( a > b )
	{
		temp = a;
		a = b;
		b = temp;
	}
	
	if( a > c )
	{
		temp = a;
		a = c;
		c = temp;
	}

	if( b > c )
	{
		temp = b;
		b = c;
		c = temp;
	}
	return b;
}

/*****************************************************************************/
ISR(USART_RXC_vect)
/*
 * Description	: Serial port interrupt. Interrupt on receiving chars from GPS
 * input		: -
 * output		: -
 * 
 * Every time a serial character from the GPS is received, this ISR is called. 
 * Received character is in UDR register. After reading this character is send
 * to handleGPS(..) function trying to decode NMEA streams. Whenever an NMEA stream
 * is decoded (correctly) the A9 is updated. Only in approx 1 second interval.
 */
{	
	if( 0 != handleGPS((char)UDR, &gpsrmc, &gpsgga) )
	{
 		if( msCnt > 50 )
 		{
 			setGPSValue(&gpsrmc, &gpsgga);
 			msCnt = 0;
 		}
	}
}

/*****************************************************************************/
ISR(TIMER0_OVF_vect)
/*
 * Description	: Timer 0 overflow ISR. Interrupt every 20ms @ 8 MHz
 * input		: -
 * output		: -
 * 
 * (1) Used to set a refresh rate for GPS updates 
 * (2) ...
 */
{
	msCnt++;
	
	TCNT0 = 0xFF - 156;
}

/*****************************************************************************/
ISR(TIMER1_CAPT_vect)
/*
 * Description	: Input capture interrupts for rpm measurements
 * input		: -
 * output		: -
 * 
 * (1) Used to set a refresh rate for GPS updates 
 * (2) ...
 */
{
	uint16_t tcNewVal = ICR1;
	
	tcidx++;
	tcidx%=8;
	
	if( tcNewVal > tcOldVal )
	{
		tcSamples[tcidx] = tcNewVal-tcOldVal;
	}
	else
	{
		tcSamples[tcidx] = 0xFFFF - tcOldVal + tcNewVal;
	}
	 
	//
	tcOldVal = tcNewVal;
}

/*****************************************************************************/
void setTemp(uint8_t nr, uint8_t value)
/*
 * Description	: Sets temperatures to A9 display.
 * input		: (uint16_t) value is temp (Celcius). 
 *			      (uint8_t) nr selects temperature 1 to 4 screen
 * output		: -
 * 
 * (1) No check on max value. 
 * (2) No check on nr. Defaults to do 'nothing'
 */
{
	uint8_t temp = value + 40;
	switch( nr )
	{
		case 1:
			pMsgWr->data[3][5] = temp;
			break;
		case 2:
			pMsgWr->data[2][5] = temp;
			break;
		case 3:
			pMsgWr->data[6][4] = temp;
			break;
		case 4:
			pMsgWr->data[6][5] = temp;
			break;
		default:
			break;
	}
}

/*****************************************************************************/
void setRpm(uint8_t nr, uint16_t value)
/*
 * Description	: Sets rpm to A9 display.
 * input		: (uint16_t) value is rpm (rev/min). 
 *			      (uint8_t) nr selects rpm 1 or 2 on A9 screen
 * output		: -
 * 
 * (1) No check on max value. 
 * (2) No check on nr. Defaults to do 'nothing'
 */
{
	unsigned char rpml = (value/10) % 0x100;
	unsigned char rpmh = (value/10) / 0x100;
	switch( nr )
	{
		case 1:
			pMsgWr->data[4][2] = rpml;
			pMsgWr->data[4][3] = rpmh;
			break;
		case 2:
			pMsgWr->data[4][4] = rpml;
			pMsgWr->data[4][5] = rpmh;
			break;
		default:
			break;
	}
}

/*****************************************************************************/
void setSpeed(uint16_t value)
/*
 * Description	: Sets GPS speed.
 * input		: (uint16_t) is speed in knots (nauctical miles/hour)
 * output		: -
 * 
 * (1) No check on max value. 
 * (2) Convert to km/h on A9 menu.
 */
{
	pMsgWr->data[3][1] = value % 0x100;
	pMsgWr->data[3][2] = value / 0x100;
}


/*****************************************************************************/
void FuelGauge(uint8_t value)
/*
 * Description	: Sets the fuel gauge on the A9 display.
 * input		: -
 * output		: -
 * 
 * (1) No check on max value. 
 */
{
	pMsgWr->data[4][1] = value;
}


/*****************************************************************************/
void setVoltage(uint16_t value)
/*
 * Description	: Display voltage on display of A9.
 * input		: (uint16_t) value holds voltage that will
 *				  be displayed * 10. E.g. 128 will display
 *                12.8V, 10 as 1.0V and 415 will display 41.5V.
 * output		: -
 * 
 * (1) A9 screen is always .2V off (fw 1.08) Compensated in code.
 * (2) No check on max value. 
 */
{
	// Compensate A9 screen. Is value equals zero A9 shows 0 -> no check
	// for zero needed.
	value -= 2;
	
	pMsgWr->data[7][1] = (uint8_t)(value & 0xFF);	// lsb	
	pMsgWr->data[7][2] = (uint8_t)(value >> 8);		// msb
}


/*****************************************************************************/
void setCurrent(uint16_t value)
/*
 * Description	: Display current (amps) on display of A9.
 * input		: (uint16_t) value holds current that will
 *				  be displayed * 10. E.g. 128 will display
 *                12.8A, 415 will display 41.5A.
 * output		: -
 * 
 * (1) See current.xlsx for current calculation
 * (2) Set currentselection to C50 (max 50A) in A9 menu
 * (3) No check on max value. 
 * (4) Rounding corrected by +0.5 when cast to uint8_t
 * (5) Small error (+/- 0.2A) when value is close to 50A
 */
{
	uint16_t val = (((value + 114.875) * 1.441)+0.5);
	
	pMsgWr->data[7][4] = (uint8_t)(val>>8);		// msb
	pMsgWr->data[7][3] = (uint8_t)(val & 0xFF);	// lsb
}


/*****************************************************************************/
void setGPSValue(GPRMC_STRUCT *gpsrmc, GPGGA_STRUCT *gpsgga)
/*
 * Description	: Copies GPS info to datablock that will be
 *				  send to Optima 7/9 on receiver's request.
 *				  (i2c slave functionallity)
 *
 *                Make sure gpsrmc and gpsggs contains valid data.
 *
 * input		: gpsrmc contains NMEA RMC data, gpsgga contains
 *				  NMEA GGA data. From GGA only altitude is used.
 * output		: -
 */
{
	// Lon and lat in nmea format DDmm.mmmm
	// Convert to A9
	char *token;
	uint16_t value;

	// get latitude DDmm.
	token = strtok(gpsrmc->latitude,".");
	value = (uint16_t) strtol(token,NULL,10);
	if(gpsrmc->NS == 'S')
	{
		value *= -1;
	}
	pMsgWr->data[1][3] = (value >> 8);
	pMsgWr->data[1][4] = (value & 0xFF);	
	
	// get latitude .mmmm
	token = strtok(NULL,".");
	value = (uint16_t) strtol(token,NULL,10);
	pMsgWr->data[1][1] = (value >> 8);
	pMsgWr->data[1][2] = (value & 0xFF);
	
	
	// get longitude DDDmm.
	token = strtok(gpsrmc->longitude,".");
	value = (uint16_t) strtol(token,NULL,10);
	if(gpsrmc->EW == 'W')
	{
		value *= -1;
	}
	pMsgWr->data[2][3] = (value >> 8);
	pMsgWr->data[2][4] = (value & 0xFF);	
	
	// get longitude .mmmm
	token = strtok(NULL,".");
	value = (uint16_t) strtol(token,NULL,10);
	pMsgWr->data[2][1] = (value >> 8);
	pMsgWr->data[2][2] = (value & 0xFF);
	
	// Set date
	pMsgWr->data[5][1] = gpsrmc->date.y;
	pMsgWr->data[5][2] = gpsrmc->date.m;
	pMsgWr->data[5][3] = gpsrmc->date.d;
	
	// Set time
	pMsgWr->data[5][4] = gpsrmc->utc.h;
	pMsgWr->data[5][5] = gpsrmc->utc.m;
	pMsgWr->data[1][5] = gpsrmc->utc.s;
	
	// Set Altitude
	value = (uint16_t) strtol(gpsgga->mslAltitude,NULL,10);
	pMsgWr->data[3][3] = (uint8_t)(value >> 8);
	pMsgWr->data[3][4] = (uint8_t)(value & 0xFF);	

	// Set course (not used on A9 display, used in PC tool)
	value = (uint16_t) strtol(gpsrmc->course,NULL,10);
	pMsgWr->data[6][1] = (uint8_t)(value >> 8);
	pMsgWr->data[6][2] = (uint8_t)(value & 0xFF);	

	// Set Ground speed,  The A9 expects km/h so multiply by 1.852
	value = (uint16_t) strtol(gpsrmc->gs,NULL,10);
	value = (uint16_t) (1.852f * value);
	pMsgWr->data[3][1] = (uint8_t)(value >> 8);
	pMsgWr->data[3][2] = (uint8_t)(value & 0xFF);	
}


/*****************************************************************************/
void i2c_init_sr(unsigned char slaveAddr)
/*
 * Description	: Initialise I2C bus (TWI in Atmel terms) for SLA+R mode.
 *
 *				  Only listen to 'slaveAddr' messages, does not respond to
 *				  general call address.
 *
 * input		: slaveAddr is I2C slave address of Hitec sensorstations
 * output		: - (ISR)
 */
{
	TWAR = 0x11; //(slaveAddr << 1);	// No general call address response
	TWDR = 0x00;
    
    TWCR = 0x00;
	TWCR = 	(1<<TWEN) |	// Enable TWI
			(1<<TWEA) | // Enable ACK
			(1<<TWIE) | // enable interrupts;
			(0<<TWSTA) | (0<<TWSTO);
}


/*****************************************************************************/
ISR(TWI_vect, ISR_BLOCK)
/*
 * Description	: I2C slave transmitter ISR. Talks SLA+R, loosely based on AVR311.
 *				  Optima 7/9 is I2C master and tries approx. every second to
 *				  'talk' to a conneced sensor station.
 *	
 *				  When no sensor station is connected, the I2C are not pulled-up
 *				  (done by sensor board) so Optima 7/9 knows no sensor board is
 *				  connected and flags A9 to hide telemetry items.
 *
 *				  When connected, Optima 7/9 initiates conversation but sensor station
 *				  decides what info to respond. In this firmware all avaliable data is
 *				  send, change to application needs.
 *				
 *				  All telemetry data is located in telemetry data structure.
 *
 * input		: - (ISR)
 * output		: - (ISR)
 */
{
	static unsigned char msgIndex = 0;
	static unsigned char msgId = 0;
	unsigned char twiStatus = TWSR & 0xF8;
	
	switch(twiStatus)
	{
		case 0xA8:	
		case 0xB0:
			msgIndex = 0;
			TWIsbusy = 1;
			//TWDR = pMsgRd->data[msgId][msgIndex++];
			TWDR = pMsgWr->data[msgId][msgIndex++];
			TWCR |= ( (1<<TWINT) | (1<<TWEA) );
			break;


		case 0xB8:
			//TWDR = pMsgRd->data[msgId][msgIndex++];
			TWDR = pMsgWr->data[msgId][msgIndex++];
			if( msgIndex < 7 )
			{
				TWCR |= ((1<<TWINT) | (1<<TWEA));
			}
			else
			{
				TWCR &=  ~(1<<TWEA);
				TWCR |= (1<<TWINT);
			}	
			break;

		case 0xC0:
		case 0xC8:
			msgId++;
			msgId%=8;
			TWIsbusy = 0;
			TWCR |= ((1<<TWINT) | (1<<TWEA));
			break;
		
		case 0xF8:
			tx('F');
			break;
			
		case 0x00:
			tx('e');
			TWCR &= ~( (1<<TWSTA) | (1<<TWEA) );
			TWCR |= ( (1<<TWINT) | (1<<TWSTO) );
			break;
		
		default:
			tx('d');
			break;
	}
}

